#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
using namespace std;

//---------------------------------------发布相关变量--------------------------------------------
ros::Publisher odometry_pose_pub;
ros::Publisher camera_pose_pub;

geometry_msgs::PoseStamped odometry_pose;
geometry_msgs::PoseStamped camera_pose;
//odometry_pose.header.frame_id = "world";
//camera_pose.header.frame_id = "world";


//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void pose_pub()
{

	odometry_pose.header.frame_id = "world";
	camera_pose.header.frame_id = "world";
	odometry_pose.header.stamp = ros::Time::now();
	camera_pose.header.stamp = ros::Time::now();

}

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void odometry_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
	/*
	if (msg->header.frame_id != "world")
	{
		cout << endl << "frame id is not world" <<endl;
		ros::shutdown;
		return 1;
	}
	*/
	odometry_pose.pose.position.x = msg->pose.pose.position.x;
	odometry_pose.pose.position.y = msg->pose.pose.position.y;
	odometry_pose.pose.position.z = msg->pose.pose.position.z;
	
	odometry_pose.pose.orientation = msg->pose.pose.orientation;

}



void camerapose_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
	/*
	if (msg->header.frame_id != "world")
	{
		cout << endl << "frame id is not world" <<endl;
		ros::shutdown;
		return 1;
	}
	*/
	
	camera_pose.pose.position.x = msg->pose.pose.position.x;
	camera_pose.pose.position.y = msg->pose.pose.position.y;
	camera_pose.pose.position.z = msg->pose.pose.position.z;
	
	camera_pose.pose.orientation = msg->pose.pose.orientation;


}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "vins_pose_pub");
	ros::NodeHandle nh("~");
	
	//
	ros::Subscriber vins_odometry = nh.subscribe<nav_msgs::Odometry>("/vins_estimator/odometry", 100, odometry_cb);
	ros::Subscriber vins_camera_pose = nh.subscribe<nav_msgs::Odometry>("/vins_estimator/camera_pose", 100, camerapose_cb);

	odometry_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/vins/odometry_pose", 100);
	camera_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/vins/camera_pose", 100);

	ros::Rate rate(20);
	
	while(ros::ok())
	{
		//回调一次 更新传感器状态
		ros::spinOnce();
		pose_pub();
		odometry_pose_pub.publish(odometry_pose);
		camera_pose_pub.publish(camera_pose);
			
		rate.sleep();
	}
	
	return 0;

}
